#include "Droplet_base.h"

// Constructors and Destructor
Droplet_base::Droplet_base(
	float x, 
	float y, 
	float a,
	btCollisionShape *colShape,
	btTransform *startTransform,
	btDiscreteDynamicsWorld *dynamicsWorld)
{
	actDat = (DropletActuatorData *)malloc(sizeof(DropletActuatorData));
	senseDat = (DropletSensorData *)malloc(sizeof(DropletSensorData));
	commDat = (DropletCommData *)malloc(sizeof(DropletCommData));
	compDat = (DropletCompData *)malloc(sizeof(DropletCompData));
	
	// Set up the bullet physics object for this Droplet
	btScalar dropletMass(1.0f);
	bool isDynamic = true;
	btVector3 localIntertia(0, 0, 0);	// Droplet starts off at 0 velocity.
	colShape->calculateLocalInertia(dropletMass, localIntertia);

	// Set the Droplet's initial position in the world. Remember that bullet uses a Y-Up coord system
	startTransform->setIdentity();
	startTransform->setOrigin(btVector3(btScalar(x), 0, btScalar(y))); // TODO : This may be wrong. Look into it!

	// Set the Droplet's angular orientation using input parameter 'a'
	btQuaternion rotationQuat;
	rotationQuat.setRotation(btVector3(btScalar(0.0), btScalar(1.0), btScalar(0.0)), btScalar(a));
	startTransform->setRotation(rotationQuat);

	btDefaultMotionState* motionState = new btDefaultMotionState(*startTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(dropletMass, motionState, colShape, localIntertia);
	actDat->physicsData = new btRigidBody(rbInfo);

	dynamicsWorld->addRigidBody(actDat->physicsData);
}
	
Droplet_base::~Droplet_base()
{
	free(actDat);
	free(senseDat);
	free(commDat);
	free(compDat);

	actDat = NULL;
	senseDat = NULL;
	commDat = NULL;
	compDat = NULL;
}

// Droplet Subsystems Setup Functions
void Droplet_base::ResetAllSystems() 
{
	
}

void Droplet_base::ResetLED() 
{ 
}

void Droplet_base::ResetCommSystem() 
{ 
}

void Droplet_base::ResetRGBSensor() 
{ 
}

void Droplet_base::ResetMotors() 
{ 
}

void Droplet_base::GetDropletID(uint16_t *id)
{
	*id = compDat->dropletID;
}

// Droplet Motion Subsystem Functions
void Droplet_base::Rotate(uint8_t dur, turn_direction dir)
{
}

void Droplet_base::Move(uint8_t dur, move_direction dir)
{
	btScalar xVel, zVel;

	switch(dir)
	{
	case NORTH : xVel = DEFAULT_DROPLET_SPEED; zVel = 0.0;
		break;
	case NORTH_EAST : xVel = SIN_DEFAULT_DROPLET_SPEED; zVel = COS_DEFAULT_DROPLET_SPEED;
		break;
	case SOUTH_EAST : xVel = -SIN_DEFAULT_DROPLET_SPEED; zVel = COS_DEFAULT_DROPLET_SPEED;
		break;
	case SOUTH : xVel = -DEFAULT_DROPLET_SPEED; zVel = 0.0;
		break;
	case SOUTH_WEST : xVel = -SIN_DEFAULT_DROPLET_SPEED; zVel = -COS_DEFAULT_DROPLET_SPEED;
		break;
	case NORTH_WEST : xVel = SIN_DEFAULT_DROPLET_SPEED; zVel = -COS_DEFAULT_DROPLET_SPEED;
		break;
	}

	actDat->physicsData->setLinearVelocity(btVector3(xVel, btScalar(0.0), zVel));
	actDat->moveTimeRemaining = static_cast<int>(dur) * 1000;	// Convert to milliseconds
}

uint8_t Droplet_base::CancelRotate() 
{
	uint8_t retval = TRUE;
	return retval;
}

uint8_t Droplet_base::CancelMove() 
{
	uint8_t retval = TRUE;
	return retval;
}

uint8_t Droplet_base::IsRotating() 
{
	uint8_t retval = TRUE;
	return retval;
}

uint8_t Droplet_base::IsMoving()
{
	uint8_t retval = TRUE;
	return retval;
}

// Droplet Simulator Functions to be overridden
//void Droplet_base::DropletInit() { return; }
//void Droplet_base::DropletMainLoop() { return; }
//void Droplet_base::DropletComplete() { return; }


// Friend Functions
void AccessActuatorData(Droplet_base *pDroplet, DropletActuatorData *actDat)
{
}

void AccessSensorData(Droplet_base *pDroplet, DropletSensorData *senseDat)
{
}

void AccessCommData(Droplet_base *pDroplet, DropletCommData *commDat)
{
}

void AccessCompData(Droplet_base *pDroplet, DropletCompData *compDat)
{
}
